#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
甘蔗农机智能管控平台 - Windows服务器
整合：双摄像头 + 激光雷达 + GPS + PLC
"""
from flask import Flask, render_template, Response, jsonify
from flask_socketio import SocketIO, emit
import socket
import threading
import json
import time
import logging

logging.basicConfig(level=logging.INFO)
logger = logging.getLogger(__name__)

app = Flask(__name__)
app.config['SECRET_KEY'] = 'gzjy_platform_2024'
socketio = SocketIO(app, cors_allowed_origins="*")

# 全局数据
current_robot = 'GZJY001'
robots = {}
robot_sockets = {}

def get_robot(robot_id):
    """获取或创建机器人数据"""
    if robot_id not in robots:
        robots[robot_id] = {
            'id': robot_id,
            'connected': False,
            'lidar': {'points': [], 'count': 0},
            'gps': {'lat': 0, 'lng': 0, 'satellites': 0, 'fix': 'No Fix'},
            'plc': {'sensors': {}, 'connected': False}
        }
    return robots[robot_id]

def send_to_robot(robot_id, command):
    """发送命令到机器人"""
    logger.info(f"[DEBUG] 尝试发送到{robot_id}, 当前sockets: {list(robot_sockets.keys())}")
    
    sock = robot_sockets.get(robot_id)
    if not sock:
        logger.warning(f"[DEBUG] robot_id={robot_id}不在sockets中！")
        return False
    
    try:
        msg = json.dumps(command) + '\n'
        logger.info(f"[DEBUG] 发送消息: {msg[:100]}")
        sock.send(msg.encode())
        logger.info(f"[命令->RK] {robot_id}: {command['type']}")
        return True
    except Exception as e:
        logger.error(f"[DEBUG] 发送失败: {e}")
        if robot_id in robot_sockets:
            del robot_sockets[robot_id]
        return False

# ========== TCP接收器 ==========

def start_control_receiver():
    """端口9000 - 状态+命令通道"""
    def receiver():
        server = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
        server.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
        server.bind(('0.0.0.0', 9000))
        server.listen(5)
        logger.info("[9000] 命令通道启动")
        
        while True:
            try:
                client, addr = server.accept()
                logger.info(f"[9000] 连接: {addr}")
                
                def handle():
                    robot_id = None
                    buffer = ""
                    
                    while True:
                        try:
                            client.settimeout(0.5)
                            data = client.recv(4096)
                            if not data:
                                break
                            
                            buffer += data.decode('utf-8')
                            while '\n' in buffer:
                                line, buffer = buffer.split('\n', 1)
                                if line.strip():
                                    msg = json.loads(line)
                                    if msg.get('type') == 'video':
                                        robot_id = msg.get('robot_id', 'UNKNOWN')
                                        logger.info(f"[DEBUG] 保存socket: robot_id={robot_id}")
                                        robot_sockets[robot_id] = client
                                        logger.info(f"[DEBUG] 当前sockets: {list(robot_sockets.keys())}")
                                        robot = get_robot(robot_id)
                                        robot['connected'] = True
                        except socket.timeout:
                            continue
                        except:
                            break
                    
                    if robot_id and robot_id in robot_sockets:
                        del robot_sockets[robot_id]
                        get_robot(robot_id)['connected'] = False
                    client.close()
                    logger.info(f"[9000] 断开: {addr}")
                
                threading.Thread(target=handle, daemon=True).start()
            except:
                pass
    
    threading.Thread(target=receiver, daemon=True).start()

def start_lidar_receiver():
    """端口9001 - 激光雷达数据"""
    def receiver():
        server = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
        server.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
        server.bind(('0.0.0.0', 9001))
        server.listen(5)
        logger.info("[9001] 雷达数据启动")
        
        while True:
            try:
                client, addr = server.accept()
                
                def handle():
                    buffer = ""
                    while True:
                        try:
                            data = client.recv(4096)
                            if not data:
                                break
                            buffer += data.decode('utf-8')
                            while '\n' in buffer:
                                line, buffer = buffer.split('\n', 1)
                                if line.strip():
                                    msg = json.loads(line)
                                    if msg.get('type') == 'lidar_parsed':
                                        robot_id = msg.get('robot_id')
                                        robot = get_robot(robot_id)
                                        robot['lidar']['points'] = msg.get('points', [])
                                        robot['lidar']['count'] = msg.get('point_count', 0)
                        except:
                            break
                    client.close()
                
                threading.Thread(target=handle, daemon=True).start()
            except:
                pass
    
    threading.Thread(target=receiver, daemon=True).start()

def start_gps_receiver():
    """端口9002 - GPS数据"""
    def receiver():
        server = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
        server.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
        server.bind(('0.0.0.0', 9002))
        server.listen(5)
        logger.info("[9002] GPS数据启动")
        
        while True:
            try:
                client, addr = server.accept()
                
                def handle():
                    buffer = ""
                    while True:
                        try:
                            data = client.recv(4096)
                            if not data:
                                break
                            buffer += data.decode('utf-8')
                            while '\n' in buffer:
                                line, buffer = buffer.split('\n', 1)
                                if line.strip():
                                    msg = json.loads(line)
                                    if msg.get('type') == 'gps_data':
                                        robot_id = msg.get('robot_id')
                                        robot = get_robot(robot_id)
                                        gps = msg.get('data', {})
                                        robot['gps']['lat'] = gps.get('latitude', 0)
                                        robot['gps']['lng'] = gps.get('longitude', 0)
                                        robot['gps']['satellites'] = gps.get('satellites', 0)
                                        robot['gps']['fix'] = gps.get('fix_type', 'No Fix')
                        except:
                            break
                    client.close()
                
                threading.Thread(target=handle, daemon=True).start()
            except:
                pass
    
    threading.Thread(target=receiver, daemon=True).start()

def start_plc_receiver():
    """端口9003 - PLC传感器数据"""
    def receiver():
        server = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
        server.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
        server.bind(('0.0.0.0', 9003))
        server.listen(5)
        logger.info("[9003] PLC数据启动")
        
        while True:
            try:
                client, addr = server.accept()
                logger.info(f"[PLC] 连接: {addr}")
                
                def handle():
                    buffer = ""
                    while True:
                        try:
                            data = client.recv(4096)
                            if not data:
                                break
                            buffer += data.decode('utf-8')
                            while '\n' in buffer:
                                line, buffer = buffer.split('\n', 1)
                                if line.strip():
                                    msg = json.loads(line)
                                    if msg.get('type') == 'sensor_data':
                                        robot_id = msg.get('robot_id')
                                        robot = get_robot(robot_id)
                                        robot['plc']['sensors'] = msg.get('sensors', {})
                                        robot['plc']['connected'] = True
                                        socketio.emit('sensor_data', {
                                            'robot_id': robot_id,
                                            'sensors': robot['plc']['sensors']
                                        })
                        except:
                            break
                    client.close()
                
                threading.Thread(target=handle, daemon=True).start()
            except:
                pass
    
    threading.Thread(target=receiver, daemon=True).start()

# ========== Web路由 ==========

@app.route('/')
def index():
    return render_template('index.html')

@app.route('/video')
def video1():
    def generate():
        while True:
            try:
                sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
                sock.settimeout(10)
                sock.connect(('192.168.10.100', 8080))
                while True:
                    data = sock.recv(16384)
                    if not data:
                        break
                    yield data
                sock.close()
                break
            except:
                time.sleep(2)
    return Response(generate(), mimetype='multipart/x-mixed-replace; boundary=frame')

@app.route('/video2')
def video2():
    def generate():
        while True:
            try:
                sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
                sock.settimeout(10)
                sock.connect(('192.168.10.100', 8081))
                while True:
                    data = sock.recv(16384)
                    if not data:
                        break
                    yield data
                sock.close()
                break
            except:
                time.sleep(2)
    return Response(generate(), mimetype='multipart/x-mixed-replace; boundary=frame')

@app.route('/api/lidar')
def api_lidar():
    robot = get_robot(current_robot)
    return jsonify({
        'points': robot['lidar']['points'],
        'point_count': robot['lidar']['count'],
        'status': 'real_data' if robot['lidar']['count'] > 0 else 'disconnected'
    })

@app.route('/api/gps')
def api_gps():
    robot = get_robot(current_robot)
    return jsonify({
        'connected': robot['gps']['lat'] != 0,
        'latitude': robot['gps']['lat'],
        'longitude': robot['gps']['lng'],
        'satellites': robot['gps']['satellites'],
        'fix_type': robot['gps']['fix']
    })

@app.route('/api/robots')
def api_robots():
    robot_list = [{'robot_id': rid, 'connected': r['connected']} for rid, r in robots.items()]
    return jsonify({'robots': robot_list, 'current': current_robot})

# ========== GIS API接口 ==========

@app.route('/api/gis/maps')
def api_gis_maps():
    """获取地图列表"""
    try:
        with open('maps/map_list.json', 'r', encoding='utf-8') as f:
            data = json.load(f)
        return jsonify(data)
    except Exception as e:
        logger.error(f"[GIS] 读取地图列表失败: {e}")
        return jsonify({'maps': []})

@app.route('/api/gis/map/<map_id>/paths')
def api_gis_map_paths(map_id):
    """获取指定地图的路径"""
    try:
        filename = f'maps/{map_id}_paths.json'
        with open(filename, 'r', encoding='utf-8') as f:
            data = json.load(f)
        return jsonify(data)
    except Exception as e:
        logger.error(f"[GIS] 读取地图路径失败: {e}")
        return jsonify({'map_id': map_id, 'paths': []})

@app.route('/api/gis/data')
def api_gis_data():
    """获取GIS数据（路径和地块）"""
    try:
        with open('gis_data.json', 'r', encoding='utf-8') as f:
            data = json.load(f)
        return jsonify(data)
    except:
        return jsonify({'paths': [], 'fields': [], 'settings': {}})

@app.route('/api/gis/paths')
def api_gis_paths():
    """获取所有路径"""
    try:
        with open('gis_data.json', 'r', encoding='utf-8') as f:
            data = json.load(f)
        return jsonify({'paths': data.get('paths', [])})
    except:
        return jsonify({'paths': []})

@app.route('/api/gis/fields')
def api_gis_fields():
    """获取所有地块"""
    try:
        with open('maps/fields.json', 'r', encoding='utf-8') as f:
            data = json.load(f)
        return jsonify(data)
    except Exception as e:
        logger.error(f"[GIS] 读取地块失败: {e}")
        return jsonify({'fields': []})

@app.route('/api/gis/field/<field_id>')
def api_gis_field(field_id):
    """获取单个地块详情"""
    try:
        with open('maps/fields.json', 'r', encoding='utf-8') as f:
            data = json.load(f)
        field = next((f for f in data['fields'] if f['id'] == field_id), None)
        if field:
            return jsonify(field)
        return jsonify({'error': 'Field not found'}), 404
    except Exception as e:
        logger.error(f"[GIS] 读取地块失败: {e}")
        return jsonify({'error': str(e)}), 500

@app.route('/api/gis/field/<field_id>/update_progress', methods=['POST'])
def api_update_field_progress(field_id):
    """更新地块作业进度"""
    try:
        from flask import request
        progress = request.json.get('progress', 0)
        
        with open('maps/fields.json', 'r', encoding='utf-8') as f:
            data = json.load(f)
        
        for field in data['fields']:
            if field['id'] == field_id:
                field['progress'] = progress
                field['last_work'] = time.strftime('%Y-%m-%d')
                if progress >= 100:
                    field['status'] = '已完成'
                elif progress > 0:
                    field['status'] = '进行中'
                break
        
        with open('maps/fields.json', 'w', encoding='utf-8') as f:
            json.dump(data, f, ensure_ascii=False, indent=2)
        
        logger.info(f"[GIS] 更新地块进度: {field_id} -> {progress}%")
        return jsonify({'success': True})
    except Exception as e:
        logger.error(f"[GIS] 更新进度失败: {e}")
        return jsonify({'success': False, 'error': str(e)})

@app.route('/api/gis/field/<field_id>', methods=['DELETE'])
def api_delete_field(field_id):
    """删除地块"""
    try:
        with open('maps/fields.json', 'r', encoding='utf-8') as f:
            data = json.load(f)
        
        data['fields'] = [f for f in data['fields'] if f['id'] != field_id]
        
        with open('maps/fields.json', 'w', encoding='utf-8') as f:
            json.dump(data, f, ensure_ascii=False, indent=2)
        
        logger.info(f"[GIS] 删除地块: {field_id}")
        return jsonify({'success': True})
    except Exception as e:
        logger.error(f"[GIS] 删除地块失败: {e}")
        return jsonify({'success': False, 'error': str(e)})

@app.route('/api/gis/create_field', methods=['POST'])
def api_create_field():
    """新建地块"""
    try:
        from flask import request
        field_data = request.json
        
        with open('maps/fields.json', 'r', encoding='utf-8') as f:
            data = json.load(f)
        
        # 生成新ID
        existing_ids = [f['id'] for f in data['fields']]
        field_num = len(existing_ids) + 1
        new_id = f"FIELD-{chr(65+field_num-1)}{field_num:02d}"
        
        field_data['id'] = new_id
        field_data['created'] = time.strftime('%Y-%m-%dT%H:%M:%SZ', time.gmtime())
        field_data['progress'] = 0
        field_data['status'] = '待作业'
        field_data['paths'] = []
        
        data['fields'].append(field_data)
        
        with open('maps/fields.json', 'w', encoding='utf-8') as f:
            json.dump(data, f, ensure_ascii=False, indent=2)
        
        logger.info(f"[GIS] 新建地块: {new_id} - {field_data['name']}")
        return jsonify({'success': True, 'field_id': new_id, 'field': field_data})
    except Exception as e:
        logger.error(f"[GIS] 新建地块失败: {e}")
        return jsonify({'success': False, 'error': str(e)})

@app.route('/api/gis/field/<field_id>/update', methods=['POST'])
def api_update_field(field_id):
    """修改地块信息"""
    try:
        from flask import request
        updates = request.json
        
        with open('maps/fields.json', 'r', encoding='utf-8') as f:
            data = json.load(f)
        
        for field in data['fields']:
            if field['id'] == field_id:
                # 更新允许修改的字段
                if 'name' in updates:
                    field['name'] = updates['name']
                if 'area' in updates:
                    field['area'] = updates['area']
                if 'center' in updates:
                    field['center'] = updates['center']
                if 'boundary' in updates:
                    field['boundary'] = updates['boundary']
                if 'crop_type' in updates:
                    field['crop_type'] = updates['crop_type']
                if 'planting_date' in updates:
                    field['planting_date'] = updates['planting_date']
                if 'paths' in updates:
                    field['paths'] = updates['paths']
                break
        
        with open('maps/fields.json', 'w', encoding='utf-8') as f:
            json.dump(data, f, ensure_ascii=False, indent=2)
        
        logger.info(f"[GIS] 更新地块: {field_id}")
        return jsonify({'success': True})
    except Exception as e:
        logger.error(f"[GIS] 更新地块失败: {e}")
        return jsonify({'success': False, 'error': str(e)})

@app.route('/api/gis/map/<map_id>/save_path', methods=['POST'])
def api_save_map_path(map_id):
    """保存路径到指定地图"""
    try:
        from flask import request
        path_data = request.json
        path_id = path_data.get('id')
        path_name = path_data.get('name', '')
        
        logger.info(f"[GIS-SAVE] ========== 收到保存请求 ==========")
        logger.info(f"[GIS-SAVE] 地图ID: {map_id}")
        logger.info(f"[GIS-SAVE] 路径ID: {path_id}")
        logger.info(f"[GIS-SAVE] 路径名: {path_name}")
        
        if not path_id:
            logger.error(f"[GIS-SAVE] 路径ID为空，拒绝保存")
            return jsonify({'success': False, 'error': '路径ID不能为空'})
        
        filename = f'maps/{map_id}_paths.json'
        
        # 读取现有数据
        try:
            with open(filename, 'r', encoding='utf-8') as f:
                data = json.load(f)
        except:
            data = {'map_id': map_id, 'map_name': '', 'paths': []}
        
        # 检查路径ID是否已存在（防止重复保存）
        existing_ids = [p['id'] for p in data['paths']]
        logger.info(f"[GIS-SAVE] 当前已有路径数: {len(data['paths'])}")
        logger.info(f"[GIS-SAVE] 现有路径IDs: {existing_ids}")
        
        if path_id in existing_ids:
            logger.warning(f"[GIS-SAVE] 路径ID已存在，拒绝重复保存: {path_id}")
            return jsonify({'success': True, 'path_id': path_id, 'map_id': map_id, 'duplicated': True})
        
        # 添加新路径
        data['paths'].append(path_data)
        logger.info(f"[GIS-SAVE] 添加新路径，当前总数: {len(data['paths'])}")
        
        # 保存路径文件
        with open(filename, 'w', encoding='utf-8') as f:
            json.dump(data, f, ensure_ascii=False, indent=2)
        logger.info(f"[GIS-SAVE] 文件已保存: {filename}")
        
        # 自动关联地块（根据路径中心点）
        if 'center' in path_data and path_data['center']:
            auto_associate_field(path_data['id'], path_data['center'])
        
        logger.info(f"[GIS-SAVE] 保存成功: {path_id} - {path_name}")
        logger.info(f"[GIS-SAVE] ========== 保存请求完成 ==========")
        return jsonify({'success': True, 'path_id': path_data['id'], 'map_id': map_id})
    except Exception as e:
        logger.error(f"[GIS-SAVE] 保存路径失败: {e}")
        import traceback
        logger.error(traceback.format_exc())
        return jsonify({'success': False, 'error': str(e)})

def auto_associate_field(path_id, path_center):
    """自动关联路径到最近的地块"""
    try:
        with open('maps/fields.json', 'r', encoding='utf-8') as f:
            data = json.load(f)
        
        # 计算距离并找到最近的地块
        min_distance = float('inf')
        nearest_field = None
        
        for field in data['fields']:
            if not field.get('center'):
                continue
            
            # 简单距离计算
            dlat = field['center'][0] - path_center[0]
            dlng = field['center'][1] - path_center[1]
            distance = dlat*dlat + dlng*dlng
            
            if distance < min_distance:
                min_distance = distance
                nearest_field = field
        
        # 关联到最近的地块
        if nearest_field and min_distance < 0.01:  # 距离阈值
            if 'paths' not in nearest_field:
                nearest_field['paths'] = []
            
            if path_id not in nearest_field['paths']:
                nearest_field['paths'].append(path_id)
                
                # 保存更新
                with open('maps/fields.json', 'w', encoding='utf-8') as f:
                    json.dump(data, f, ensure_ascii=False, indent=2)
                
                logger.info(f"[GIS] 路径 {path_id} 自动关联到地块 {nearest_field['id']}")
    except Exception as e:
        logger.error(f"[GIS] 自动关联失败: {e}")

@app.route('/api/gis/save_path', methods=['POST'])
def api_save_path():
    """保存新路径（兼容旧接口）"""
    try:
        from flask import request
        path_data = request.json
        map_id = path_data.get('map_id', 'MAP-001')
        return api_save_map_path(map_id)
    except Exception as e:
        logger.error(f"[GIS] 保存路径失败: {e}")
        return jsonify({'success': False, 'error': str(e)})

@app.route('/api/gis/map/<map_id>/delete_path/<path_id>', methods=['DELETE'])
def api_delete_map_path(map_id, path_id):
    """从指定地图删除路径"""
    try:
        filename = f'maps/{map_id}_paths.json'
        
        with open(filename, 'r', encoding='utf-8') as f:
            data = json.load(f)
        
        data['paths'] = [p for p in data['paths'] if p['id'] != path_id]
        
        with open(filename, 'w', encoding='utf-8') as f:
            json.dump(data, f, ensure_ascii=False, indent=2)
        
        logger.info(f"[GIS] 从{map_id}删除路径: {path_id}")
        return jsonify({'success': True})
    except Exception as e:
        logger.error(f"[GIS] 删除路径失败: {e}")
        return jsonify({'success': False, 'error': str(e)})

# ========== WebSocket事件 ==========

@socketio.on('connect')
def handle_connect(auth=None):
    logger.info("[Web] 客户端连接")

@socketio.on('movement_control')
def handle_movement(data):
    send_to_robot(current_robot, {
        'type': 'movement',
        'forward': data.get('forward', 0),
        'turn': data.get('turn', 0)
    })

@socketio.on('control_command')
def handle_control(data):
    send_to_robot(current_robot, {
        'type': 'control',
        'control_type': data.get('type'),
        'value': data.get('value', 0)
    })

@socketio.on('continuous_forward_test')
def handle_test(data):
    send_to_robot(current_robot, {
        'type': 'continuous_forward',
        'duration': data.get('duration', 5000)
    })

@socketio.on('assign_task')
def handle_assign_task(data):
    """处理任务分配"""
    robot_id = data.get('robot_id')
    path_id = data.get('path_id')
    path_name = data.get('path_name', '')
    path_points = data.get('path_points', [])
    mode = data.get('mode', 'auto')
    
    logger.info(f"[任务分配] 机器人: {robot_id}, 路径: {path_name} ({path_id}), 模式: {mode}")
    logger.info(f"[任务分配] 路径点数: {len(path_points)}")
    
    # 发送任务给机器人
    task_command = {
        'type': 'path_task',
        'path_id': path_id,
        'path_name': path_name,
        'path_points': path_points,
        'mode': mode
    }
    
    success = send_to_robot(robot_id, task_command)
    
    if success:
        logger.info(f"[任务分配] 任务已发送给 {robot_id}")
        emit('task_assigned', {'success': True, 'robot_id': robot_id, 'path_name': path_name})
    else:
        logger.error(f"[任务分配] 发送失败，机器人 {robot_id} 未连接")
        emit('task_assigned', {'success': False, 'error': '机器人未连接'})

if __name__ == '__main__':
    logger.info("=" * 50)
    logger.info("甘蔗农机智能管控平台 - Windows服务器")
    logger.info("=" * 50)
    logger.info("Web: http://192.168.10.1:5000")
    logger.info("=" * 50)
    
    start_control_receiver()
    start_lidar_receiver()
    start_gps_receiver()
    start_plc_receiver()
    
    socketio.run(app, host='0.0.0.0', port=5000, debug=False)

